#ifndef RM_AIM__BALLISTIC_SOLVER_NODE_HPP_
#define RM_AIM__BALLISTIC_SOLVER_NODE_HPP_

#include "rclcpp/rclcpp.hpp"
#include "aim_interfaces/msg/target.hpp"
#include "aim_interfaces/msg/ballistic_cmd.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "aim_interfaces/msg/imu.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include <cmath>
#include <vector>
#include <string>
#include <utility>
#include <limits>

#include "ballistic_solver/ballistic_solver_core.hpp"

namespace rm_aim
{
    class BallisticSolverNode : public rclcpp::Node
    {
    public:
        /**
         * @brief 弹道计算节点
         * @param options 节点参数
         */
        BallisticSolverNode(const rclcpp::NodeOptions &options);

    private:
        // -------------------------- 回调函数声明 --------------------------
        /**
         * @brief 订阅跟踪目标数据回调
         * @param msg 跟踪目标数据
         */
        void trackerCallback(const aim_interfaces::msg::Target::SharedPtr msg);

        // -------------------------- 核心逻辑函数声明 --------------------------
        /**
         * @brief 弹道计算逻辑
         */
        void process_ballistic();

        /**
         * @brief 计算精确弹道目标点
         * @param target 跟踪目标数据
         * @return 精确弹道目标点
         */
        geometry_msgs::msg::Point calculatePreciseAimPoint(const aim_interfaces::msg::Target &target);



        // -------------------------- 辅助函数声明 --------------------------

        /**
         * @brief 声明 ROS 所有参数（调用 SDK 接口获取默认值和范围）
         */
        void declareAllParameters();

        /**
         * @brief 创建浮点参数描述
         * @param description 参数描述
         * @param min_val 参数最小值
         * @param max_val 参数最大值
         * @param step 参数步进
         * @return 创建的参数描述
         */
        rcl_interfaces::msg::ParameterDescriptor createFloatParamDesc(
            const std::string &description,
            double min_val, double max_val, double step);
        // -------------------------- 成员变量声明 --------------------------
        double bullet_speed_; // 子弹速度
        double k1_;           // 空气阻力系数
        double gravity_;      // 重力加速度
        double system_delay_; // 系统延迟
        BallisticSolverCore ballistic_solver_;  // 弹道计算核心
        OnSetParametersCallbackHandle::SharedPtr params_callback_handle_; // 参数回调句柄
        std::atomic<bool> can_outpost_shoot_;  // 前哨站是否可发射（原子变量保证线程安全）
        // -------------------------- 延迟参数成员变量 --------------------------
        // Normal模式
        double normal_fixed_delay_;       // 固定延迟（s）
        double normal_prediction_weight_; // 飞行时间权重
        // Outpost模式
        double outpost_angle_delay_scale_; // 角度延迟缩放系数
        double outpost_time_bias_;         // 固定偏置（s）
        double outpost_min_delay_;         // 最小延迟（s）
        // GreenLight模式
        double green_light_fixed_delay_;        // 固定延迟（s）
        double green_light_flight_time_weight_; // 飞行时间权重

        aim_interfaces::msg::Target tracker_data_; // 跟踪目标数据（含弧度制相对Yaw）
        aim_interfaces::msg::Imu imu_data_;        // IMU数据缓存（四元数姿态）
        std::string current_mode_;                 // 当前模式（从tracker id解析）
        double mode_delay;                         // 当前模式延迟

        // -------------------------- 动态参数回调函数 --------------------------
        rcl_interfaces::msg::SetParametersResult onParametersChanged(
            const std::vector<rclcpp::Parameter> &parameters);

        // ROS 订阅者/发布者
        rclcpp::Subscription<aim_interfaces::msg::Target>::SharedPtr tracker_sub_;
        rclcpp::Publisher<aim_interfaces::msg::BallisticCmd>::SharedPtr ballistic_pub_;
    };
} // namespace rm_aim

#endif // RM_AIM__BALLISTIC_SOLVER_NODE_HPP_